//
// Created by fanghr on 17-12-24.
//

#include "Serial.h"
#include "imu_data_parser.h"
#include "../include.h"

void cb(char *buf, size_t) {
    for (int i = 0; i < 120; i++) {
        if ((uint16_t) buf[i] == 0x55) {
            while (i + 11 < 120) {
                boost::array<uint8_t, 11> tmp;
                for (int p = 0; p < 11; p++)
                    tmp.at(p) = buf[i + p];
                boost::array<uint8_t, 9> tmp2;
                for (int q = 0; q < 9; q++)
                    tmp2.at(q) = tmp.at(1 + q);
                (imu_data_parser(tmp2));
                i = i + 11;
            }
        }
    }
}

int imu_thread(boost::asio::io_service &io, std::string device) {
    try {
        Serial serial(io, device);
        //serial.set_baud_rate(9600);
        serial.set_callback(cb);
        serial.run();
    }
    catch (boost::exception_detail::clone_impl<boost::exception_detail::error_info_injector<boost::system::system_error> >) {
        return 0;
    }
    return 0;
}